#include <string.h>
#include <stdio.h>
#include "mhscpu.h"
#include "mh_sm4.h"
#include "debug.h"
#include "mh_bignum_tool.h"

void printf_bytes(uint8_t *buf, uint32_t size)
{
	uint32_t i;
	for(i = 0; i < size; i++)
	{
		DBG_PRINT("%02X ", buf[i]);
	}
	DBG_PRINT("\n");
}

void test_SM4(void)
{	
	uint32_t t;
	uint8_t Key[16] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0xfe, 0xdc, 0xba, 0x98, 0x76, 0x54, 0x32, 0x10};
	uint8_t Iv[16] = {0xA3, 0xB1, 0xBA, 0xC6, 0x56, 0xAA, 0x33, 0x50, 0x67, 0x7D, 0x91, 0x97, 0xB2, 0x70, 0x22, 0xDC};
	uint8_t Plain[16] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0xfe, 0xdc, 0xba, 0x98, 0x76, 0x54, 0x32, 0x10};
	uint8_t Crypt[16] = {0x68, 0x1E, 0xDF, 0x34, 0xD2, 0x06, 0x96, 0x5E, 0x86, 0xB3, 0xE9, 0x4F, 0x53, 0x6E, 0x42, 0x46};
	uint8_t mPlain[16];
	uint8_t mCrypt[16];

//	uint8_t Crypt_1M[32] = {0x59, 0x52, 0x98, 0xc7, 0xc6, 0xfd, 0x27, 0x1f, 0x04, 0x02, 0xf8, 0x04, 0xc3, 0x3d, 0x3f, 0x66};


	uint8_t xPlain[2][32] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0xfe, 0xdc, 0xba, 0x98, 0x76, 0x54, 0x32, 0x10};
	uint8_t xCrypt[32];

	DBG_PRINTF("\nSM4 Test In\n");

	//ECB
	memset(mCrypt, 0, sizeof(mCrypt));
	memset(mPlain, 0, sizeof(mPlain));
	
	mh_sm4_enc(ECB, mCrypt, sizeof(mCrypt), Plain, sizeof(Plain), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(mCrypt, Crypt, sizeof(Crypt)));
	r_printf(t, "SM4 ECB Encrypt Test\n");
	memset(mCrypt,0,sizeof(mCrypt));
	mh_sm4_dec(ECB, mPlain, sizeof(mPlain), Crypt, sizeof(Crypt), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(mPlain, Plain, sizeof(Plain)));
	r_printf(t, "SM4 ECB Decrypt Test\n");
	memset(mPlain,0,sizeof(mPlain));

	
	//CBC
	memset(mCrypt, 0, sizeof(mCrypt));
	memset(mPlain, 0, sizeof(mPlain));

	mh_sm4_enc(CBC, mCrypt, sizeof(mCrypt), Plain, sizeof(Plain), Key, Iv, NULL, NULL);
	mh_sm4_dec(CBC, mPlain, sizeof(mPlain), mCrypt, sizeof(mCrypt), Key, Iv, NULL, NULL);
	
//	mh_sm4_enc(CBC, mCrypt, sizeof(mCrypt), Plain, sizeof(Plain), Key, Iv, mh_rand_p, NULL);
//	mh_sm4_dec(CBC, mPlain, sizeof(mPlain), mCrypt, sizeof(mCrypt), Key, Iv, mh_rand_p, NULL);
	
	t = (!memcmp(mPlain, Plain, sizeof(Plain)));
	r_printf(t, "SM4 CBC Test\n");
	
	
	//xCBC
	memset(xCrypt, 0, sizeof(xCrypt));
// 	memset(xPlain, 0, sizeof(xPlain));
// 	
// 	mh_rand(xPlain[0], sizeof(xPlain[0]));
	
	mh_sm4_enc(CBC, xCrypt, sizeof(xCrypt), xPlain[0], sizeof(xPlain[0]), Key, Iv, mh_rand_p, NULL);
	mh_sm4_dec(CBC, xPlain[1], sizeof(xPlain[1]), xCrypt, sizeof(xCrypt), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(xPlain[1], xPlain[0], sizeof(xPlain[1])));
	r_printf(t, "SM4 CBC xbytes Test\n");
}
void mh_SM4_test_with_known_data0(void);
void mh_SM4_test_with_known_data1(void);
void mh_SM4_test_with_known_data2(void);

void mh_SM4_test_with_known_data(void)
{
	mh_SM4_test_with_known_data0();
	mh_SM4_test_with_known_data1();
	mh_SM4_test_with_known_data2();
}

void mh_SM4_test_with_known_data0(void)
{
	uint32_t t;
	uint8_t Plain[16 * 4] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
							0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,
							0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,
							0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f};
	uint8_t Key[16] = {0xb1,0x22,0xad,0x0a,0x7a,0x36,0x2e,0xc3,0xab,0xa1,0xdd,0xef,0xb3,0xaf,0x49,0x15};
	uint8_t Iv[16] = {0x11,0x11,0x11,0x11,0xd1,0xa1,0x6c,0x20,0x67,0x70,0x8a,0xcb,0xd1,0xa1,0x6c,0x20};
	uint8_t Crypt_ECB[16 * 4] = {0xab,0x54,0xe1,0x1d,0x8b,0xc0,0xde,0x81,0x35,0x3e,0x41,0x0c,0x45,0x0e,0xd8,0xc1,0x09,0x2f,0xde,0xb9,0x83,0xf4,0xe0,0x0e,0x69,0x54,0x95,0x18,0x7d,0xcc,0xfb,0x93,0xfb,0xbd,0xda,0xa6,0x35,0x4f,0xaa,0xf9,0xe6,0x62,0xab,0x62,0x2b,0x31,0x02,0x78,0x05,0x59,0xcf,0x0d,0xa0,0x3a,0x54,0xbf,0x79,0xb8,0xeb,0xb6,0x08,0xce,0x36,0x30};
	uint8_t Crypt_CBC[16 * 4] = {0x59,0x40,0x8a,0x35,0x1e,0xd4,0xe0,0x0d,0xa6,0xc3,0xf9,0xc2,0x27,0xa9,0x73,0x8d,0x07,0xbf,0x44,0x4d,0x66,0x79,0xbd,0x5c,0x87,0xda,0x21,0xb5,0xac,0xc5,0x4a,0x39,0x40,0x26,0xdc,0x35,0x70,0x57,0x46,0x69,0xb7,0x15,0x55,0xc0,0xf3,0x2a,0xcb,0x71,0x4c,0x15,0x20,0xcd,0x77,0xce,0xeb,0x1a,0x16,0x67,0x0a,0x63,0x4a,0xdf,0x46,0x29};

	uint8_t Crypt2[16 * 4] = {0};
	uint8_t Plain2[16 * 4] = {0};

	DBG_PRINTF("\n%s\n", __func__);

	//ECB
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
	
	mh_sm4_enc(ECB, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Crypt_ECB, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 ECB Encrypt Test\n");

	mh_sm4_dec(ECB, Plain2, sizeof(Plain2), Crypt_ECB, sizeof(Crypt_ECB), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 ECB Decrypt Test\n");

	
	//CBC
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
			
	mh_sm4_enc(CBC, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Crypt_CBC, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 CBC Encrypt Test\n");
	
	mh_sm4_dec(CBC, Plain2, sizeof(Plain2), Crypt_CBC, sizeof(Crypt_CBC), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 CBC Decrypt Test\n");
}


void mh_SM4_test_with_known_data1(void)
{
	uint32_t t;
	uint8_t Plain[16 * 4] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
							0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,
							0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,
							0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f};
	uint8_t Key[16] = {0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,0x10};
	uint8_t Iv[16] = {0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,0x10};
	uint8_t Crypt_ECB[16 * 4] = {0x6c,0x1d,0xd6,0xaf,0xca,0xa9,0x43,0x1e,0x4c,0x26,0xb3,0xcd,0xde,0x0a,0x73,0x24,0xd7,0xc6,0x4c,0x78,0x75,0x79,0x83,0xf8,0xb8,0xa2,0x92,0x19,0xf0,0x90,0x5c,0x9e,0x31,0xc2,0xe7,0xfb,0xcb,0x22,0x67,0x3f,0xd5,0xa1,0xcb,0x44,0xa7,0x4b,0x04,0x1b,0x2e,0xe8,0xdc,0xe4,0x63,0x41,0xf0,0x08,0xe6,0xfe,0x1c,0x1a,0x7b,0x14,0x2a,0x30};
	uint8_t Crypt_CBC[16 * 4] = {0xcc,0x86,0x86,0x4b,0xb7,0x9f,0x4e,0x58,0xba,0x83,0x3f,0xb4,0x87,0x21,0x39,0xfd,0x5d,0x04,0x2a,0xa3,0x27,0x29,0x72,0x28,0x76,0x72,0x56,0x60,0x2f,0x47,0x1a,0xc8,0xf9,0x38,0xab,0x9e,0x25,0x4a,0xbc,0x74,0x2c,0x68,0xda,0xd8,0x0f,0x7a,0xa5,0x30,0x5a,0x64,0x03,0xd5,0x93,0x87,0x71,0x15,0x8f,0x49,0x0d,0x52,0x80,0xb8,0x18,0xe6};

	uint8_t Crypt2[16 * 4] = {0};
	uint8_t Plain2[16 * 4] = {0};

	DBG_PRINTF("\n%s\n", __func__);

	//ECB
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
	
	mh_sm4_enc(ECB, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Crypt_ECB, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 ECB Encrypt Test\n");

	mh_sm4_dec(ECB, Plain2, sizeof(Plain2), Crypt_ECB, sizeof(Crypt_ECB), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 ECB Decrypt Test\n");

	
	//CBC
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
			
	mh_sm4_enc(CBC, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Crypt_CBC, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 CBC Encrypt Test\n");
	
	mh_sm4_dec(CBC, Plain2, sizeof(Plain2), Crypt_CBC, sizeof(Crypt_CBC), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 CBC Decrypt Test\n");
}

void mh_SM4_test_with_known_data2(void)
{
	uint32_t t;
	uint8_t Plain[16 * 4] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
							0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,
							0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,
							0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f};
	uint8_t Key[16] = {0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0xab,0xa1,0xdd,0xef,0xb3,0xaf,0x49,0x15};
	uint8_t Iv[16] = {0x11,0x11,0x11,0x11,0xd1,0xa1,0x6c,0x20,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,0x10};
	uint8_t Crypt_ECB[16 * 4] = {0x59,0x0b,0xf4,0xfe,0xc7,0x08,0xdb,0x75,0x62,0xca,0x73,0xe3,0xc1,0x5f,0x51,0x35,0x2f,0xb4,0xf3,0x27,0xdd,0xb5,0x54,0xcd,0x75,0xb9,0x9e,0x2c,0x2e,0x45,0x85,0xee,0xc3,0xb7,0xe6,0x45,0xe3,0x94,0x34,0x74,0xbc,0x95,0x72,0x9e,0xe2,0x3b,0xb9,0x19,0xe7,0xd0,0x5b,0x0a,0x57,0x8d,0x8b,0x5b,0x9c,0x0c,0x87,0x5a,0x1a,0x7c,0xa6,0xd2};
	uint8_t Crypt_CBC[16 * 4] = {0x21,0x37,0xe3,0x20,0x95,0xcf,0x11,0x5f,0x4f,0x49,0xf6,0x84,0xfd,0xe2,0x43,0x69,0x9f,0xb5,0xb9,0x4f,0xae,0x41,0xbf,0x39,0x94,0x46,0x8e,0x81,0x0a,0x8e,0xf4,0xb2,0x0f,0xbf,0x6e,0x0c,0x4c,0x89,0x5b,0xe2,0x0c,0x86,0xb5,0x6c,0x89,0x9a,0x10,0x77,0xb9,0x61,0x67,0xb7,0x19,0x50,0x39,0xc0,0x70,0xfb,0xb7,0x33,0xb0,0x1f,0xee,0xdb};

	uint8_t Crypt2[16 * 4] = {0};
	uint8_t Plain2[16 * 4] = {0};

	DBG_PRINTF("\n%s\n", __func__);

	//ECB
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
	
	mh_sm4_enc(ECB, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Crypt_ECB, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 ECB Encrypt Test\n");

	mh_sm4_dec(ECB, Plain2, sizeof(Plain2), Crypt_ECB, sizeof(Crypt_ECB), Key, NULL, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 ECB Decrypt Test\n");

	
	//CBC
	memset(Plain2, 0, sizeof(Plain2));
	memset(Crypt2, 0, sizeof(Crypt2));
			
	mh_sm4_enc(CBC, Crypt2, sizeof(Crypt2), Plain, sizeof(Plain), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Crypt_CBC, Crypt2, sizeof(Crypt2)));
	r_printf(t, "SM4 CBC Encrypt Test\n");
	
	mh_sm4_dec(CBC, Plain2, sizeof(Plain2), Crypt_CBC, sizeof(Crypt_CBC), Key, Iv, mh_rand_p, NULL);
	t = (!memcmp(Plain2, Plain, sizeof(Plain)));
	r_printf(t, "SM4 CBC Decrypt Test\n");
}


